#include <tasks_manager/task_manager_node.h>

TaskManagerNode::TaskManagerNode(tf2_ros::Buffer* tf_buffer):
    __tf_buffer(tf_buffer),__map_info_ptr(nullptr),__hardware_state_ptr(nullptr),__map_status_ptr(nullptr),
    __task_echo_ptr(nullptr),__curr_task_single(nullptr),__map_list_ptr(nullptr){
    // 初始化日志发布
    __log_pub = __nh.advertise<cln_msgs::LogMsg>("/log_save", 200);
    ros::Duration(1.0).sleep();
    // 参数检测
    __robot_safe = false;
    __robot_lock_state = false;
    __week_days = {"Monday","Tuesday","Wednesday","Thursday","Friday","Saturday","Sunday"};
    __translation_key_list = {"cn", "en", "tc", "es", "fr", "ko", "ja", "de"};
    string package_path = ros::package::getPath("tasks_manager");
    __report_config_yaml_file = package_path + "/config/task_report_params.yaml";
    UpdateTaskReportConfig();
    ros::param::param<std::string>("/db_dir", __db_dir, std::string("/home/aoting/user_config/db/"));
    ros::param::param<std::string>("/cache_dir", __cache_dir, std::string("/home/aoting/user_config/cache_database/"));
    ros::param::param<bool>("/path_gen_use_cln_msgs", __path_gen_use_cln_msgs, false);
    ros::param::param<std::string>("/tasks_db", __tasks_db, std::string("/home/aoting/user_config/db/tasks_db.db"));
    ros::param::param<std::string>("/tasks_record_db", __tasks_record_db, std::string("/home/aoting/user_config/db/tasks_record_db.db"));
    __all_finished_doing = ros::param::param<int>("/cleantask/all_finished", 1);   // 所有工作结束选项,0 - 停在原地,1 - 回充电桩,3 - 回停靠点
    __maps_dir = "/home/aoting/user_config/map_database";
    cout << "path_gen_use_cln_msgs: " << (__path_gen_use_cln_msgs ? "true" : "false") << endl;
    ros::param::param<bool>("/working_voice/enable", __work_voice.enable, false);
    ros::param::param<std::string>("/working_voice/data", __work_voice.voice_data, "努力工作中，请不要打扰");
    ros::param::param<int>("/working_voice/frequency", __work_voice.frequency, 60);
    ros::param::param<int>("/timertask/cancel_charge", __timertask_cancel_charge, 90);     //充电中电量判断
    ros::param::param<int>("/timertask/no_dc_charge", __timertask_no_dc_charge, 20);      //非充电中电量判定
    ros::param::param<int>("/percentage_gohome", __power_gohome, 20);                      //低电量
    // 初始化数据库
    if(!file_manager::IsExists(__db_dir)){
        file_manager::CreateDir(__db_dir);
    }
    __task_db_ptr = boost::make_shared<TaskDB>(__tasks_db, __tasks_record_db);
    __task_db_ptr->CreateTables();
    // 兼容数据处理
    TaskDataCompatible(__cache_dir);

    // 初始ROS话题服务
    // Publisher
    __voice_pub = __nh.advertise<cln_msgs::VoiceOutput>("/voice_tts", 10);
    __alarm_event_pub = __nh.advertise<cln_msgs::AlarmMsg>("/alarm_event", 100);
    __online_task_single_pub = __nh.advertise<v3_msgs::TaskSingle>("/online_task_single", 10);
    __online_task_group_pub = __nh.advertise<v3_msgs::TaskGroup>("/online_task_group", 10);
    __online_tasks_deque_pub = __nh.advertise<v3_msgs::OnlineTask>("/online_tasks_deque", 10, true);
    // __notification_record_upload = __nh.advertise<std_msgs::String>("/upload_taskgroup_id", 10, true);
    // Subscriber
    __map_name_sub = __nh.subscribe("/map_name", 1, &TaskManagerNode::MapNameSub, this);
    __map_info_sub = __nh.subscribe("/map_info_v3", 1, &TaskManagerNode::MapInfoSub, this);
    __p2p_move_sub = __nh.subscribe("/p2p_move", 10, &TaskManagerNode::P2PMoveSub, this);
    __go_home_sub = __nh.subscribe("/go_home", 10, &TaskManagerNode::GoHomeSub, this);
    __onlinetask_ctrl_sub = __nh.subscribe("/onlinetask_ctrl", 255, &TaskManagerNode::OnlineTaskCtrlSub, this);    
    __task_echo_sub = __nh.subscribe("/task_echo_v3", 1, &TaskManagerNode::TaskEchoSub, this);
    __robot_safe_sub = __nh.subscribe("/robot_safe", 1, &TaskManagerNode::RobotSafeSub, this);
    __hardware_state_sub = __nh.subscribe("/hardware_state", 1, &TaskManagerNode::HardwareStateSub, this);
    __map_status_sub = __nh.subscribe("/map_status", 1, &TaskManagerNode::MapStatusSub, this);
    __params_update_sub = __nh.subscribe("/robot_param_change", 1, &TaskManagerNode::ParamsUpdateSub, this);
    __map_list_sub =  __nh.subscribe("/map_list", 1, &TaskManagerNode::MapListSub, this);
    // ServiceServer
    __task_generate_server = __nh.advertiseService("/task_generate_server", &TaskManagerNode::TaskGenerateServer, this);
    __task_single_server = __nh.advertiseService("/task_manager_server", &TaskManagerNode::TaskSingleServer, this);
    __task_group_server = __nh.advertiseService("/taskgroup_manager_server", &TaskManagerNode::TaskGroupServer, this);
    __timer_task_server = __nh.advertiseService("/timertask_manager_server", &TaskManagerNode::TimerTaskServer, this);
    __task_finished_record_server = __nh.advertiseService("/task_finished_record_server", &TaskManagerNode::TaskFinishedRecordServer, this);
    __taskgroup_record_server = __nh.advertiseService("/taskgroup_record_server", &TaskManagerNode::TaskGroupReportServer, this);
    // ServiceClient
    __task_controller_client = __nh.serviceClient<v3_msgs::TaskControllerManager>("/task_controller_server");
    __report_record_client = __nh.serviceClient<v3_msgs::TaskReportManager>("/report_record_server");
    __map_info_client = __nh.serviceClient<v3_msgs::MapInfoRequest>("/map_info_server");
    __cloudsync_record_client = __nh.serviceClient<v3_msgs::CloudSyncTaskReportManager>("/cloudsync_task_report_server");
    // 设置线程数据
    std::thread timertask_running_thread(&TaskManagerNode::TimerTaskRunning, this);
    timertask_running_thread.detach();

    // 更新任务队列数据, 并发布数据
    OnlineTaskDequeUpdatePub();
    TasksMonitor(); //任务状态监控函数
}

TaskManagerNode::~TaskManagerNode(){

}

// 日志发布
void TaskManagerNode::LogPub(string log_level, string msg){
    __log_save.LogPub_ClnMsg(__log_pub, log_level, msg);
}

// 日志发布
void TaskManagerNode::LogPub(string log_level, vector<string> msgs){
    __log_save.LogPub_ClnMsg(__log_pub, log_level, msgs);
}

// 参数更新订阅函数
void TaskManagerNode::ParamsUpdateSub(const cln_msgs::StringArrayConstPtr& msg){
    for(auto update_str : msg->data){
        if(update_str == "/robot_lock_state"){      //更新锁机状态
            __robot_lock_state = ros::param::param<int>("/robot_lock_state",0);      //锁机状态  1 锁机 0 解锁 ft.wang 20230524
        }
        if(update_str == "/working_voice"){      //更新语音
            ros::param::param<bool>("/working_voice/enable", __work_voice.enable, false);
            ros::param::param<std::string>("/working_voice/data", __work_voice.voice_data, "努力工作中，请不要打扰");
            ros::param::param<int>("/working_voice/frequency", __work_voice.frequency, 60);
        }
        if (update_str == "/timertask"){
            ros::param::param<int>("/timertask/cancel_charge", __timertask_cancel_charge, 90);     //充电中电量判断
            ros::param::param<int>("/timertask/no_dc_charge", __timertask_no_dc_charge, 20);      //非充电中电量判定
        }
    }    
}

// 更新任务报告参数配置
void TaskManagerNode::UpdateTaskReportConfig(){
    if(__report_config_yaml_file.empty()){        //校验参数文件
        return;
    }
    YAML::Node config = file_manager::ReadYamlFile(__report_config_yaml_file);
    if(!config.IsNull()){
        if(config["task_report_params"]){
            YAML::Node params_config = config["task_report_params"];
            // coverage_threshold
            if(params_config["coverage_threshold"]){
                __task_report_config.coverage_threshold = params_config["coverage_threshold"].as<int>();
            }
            // report_reasons_translation
            if(params_config["report_reasons_translation"]){
                YAML::Node reasons_translation_config = params_config["report_reasons_translation"];
                for(YAML::const_iterator it = reasons_translation_config.begin(); it != reasons_translation_config.end(); it++){
                    string item_key = it->first.as<string>();
                    // cout << "item_key: " << item_key << " " << it->second << endl;
                    if(it->second["code"]){
                        long error_code = it->second["code"].as<long>();
                        vector<string> translation_list;
                        for(string translation_key :  __translation_key_list){
                            string error_msg = it->second[translation_key] ? it->second[translation_key].as<string>() : "";
                            translation_list.emplace_back(error_msg);
                        }
                        __task_report_config.reasons_translation_list[error_code] = translation_list;
                        __task_report_config.error_code_map[item_key] = error_code;
                    }
                }
            }
        }				
    }

    // 打印数据
    cout << "[UpdateTaskReportConfig] coverage_threshold: " << __task_report_config.coverage_threshold 
        << ", reasons_translation_list size: " << __task_report_config.reasons_translation_list.size() << endl;
    for(auto it = __task_report_config.reasons_translation_list.begin(); it != __task_report_config.reasons_translation_list.end(); it++)
        cout << "[UpdateTaskReportConfig] error_code: " << it->first
            << ", translation_list size: " << it->second.size()
            << ", front_data: " << (it->second.size() > 0 ? it->second.front().c_str() : "")
            << endl;
}


int main(int argc, char** argv){
    ros::init(argc, argv, "tasks_manager_node");
    setlocale(LC_ALL,"");   //ROS_INFO 中文显示, 需要添加头文件#include <sstream>
	
    // Debug 调试
    // if( ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Debug) ) {
    //     ros::console::notifyLoggerLevelsChanged();
    // }

    ROS_INFO("tasks_manager_node initializating......");
    ros::NodeHandle nh;
    tf2_ros::Buffer tfBuffer(::ros::Duration(10));
    tf2_ros::TransformListener tfListener(tfBuffer);

    TaskManagerNode task_manager_node(&tfBuffer);

    ros::spin();
    return 0;
}

